Multivariate KF Examples

Example 9 – vehicle location estimation

We design a six-dimensional Kalman Filter without control input. That is, we estimate a vehicle’s location on the XY plane. The vehicle has an on-board location sensor that reports X and Y coordinates of the system. We assume constant acceleration dynamics.

Kalman Filter equations

There is no control input so: \(\textbf{u} = 0\)

The state extrapolation equation: \[\hat{\textbf{x}}_{n+1, n} = \textbf{F} \hat{\textbf{x}}_{n, n}\]

The system state \(x_{n}\) is defined by: \[ \textbf{x}_{n} = \begin{bmatrix} x_{n} \\ \dot{x}_{n} \\ \ddot{x}_{n} \\ y_{n} \\ \dot{y}_{n} \\ \ddot{y}_{n} \end{bmatrix} \]

So the extrapolated vehicle state for time n + 1 can be described by:

\(\begin{array}{ccc} \begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \\ \hat{\ddot{x}}_{n+1,n} \\ \hat{y}_{n+1,n} \\ \hat{\dot{y}}_{n+1,n} \\ \hat{\ddot{y}}_{n+1,n} \end{bmatrix} = & \begin{bmatrix} 1 & \Delta t & 0.5 \Delta t^2 & 0 & 0 & 0\\ 0 & 1 & \Delta t & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t & 0.5 \Delta t^2\\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} & \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \\ \hat{\ddot{x}}_{n,n} \\ \hat{y}_{n,n} \\ \hat{\dot{y}}_{n,n} \\ \hat{\ddot{y}}_{n,n} \end{bmatrix} \end{array}\)

The covariance extrapolation equation:

The estimate covariance matrix is, assuming that the estimation error in \(X\) and \(Y\) axes are not correlated: \[ P = \begin{bmatrix} p_{x} & p_{x\dot{x}} & p_{x\ddot{x}} & 0 & 0 & 0 \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ 0 & 0 & 0 & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{bmatrix} \]

The process noise matrix, assuming a discrete noise model: \[\begin{align*} Q = & \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & 0 & 0 & 0\\ 0 & 0 & 0 & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \\ \end{bmatrix} \\ &= \sigma^2_{a} \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\ \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\ \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\ 0 & 0 & 0 & \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t \\ 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \\ \end{bmatrix} \end{align*}\]

The measurement equation:

The measurement provides us only \(X\) and \(Y\) coordinates of the vehicle: \[ z_{n} = \begin{bmatrix} x_{n, measured} \\ y_{n, measured} \end{bmatrix} \]

The dimension of \(z_{n}\) is \(2×1\) and the dimension of \(x_{n}\) is \(6×1\). Therefore the dimension of the observation matrix \(H\) shall be \(2 × 6\). \[ \textbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \]

The measurement uncertainty, assuming that the \(x\) and \(y\) measurements are uncorrelated: \[ \textbf{R}_{n} = \begin{bmatrix} \sigma^2_{x_{m}} & 0 \\ 0 & \sigma^2_{y_{m}} \end{bmatrix} \]

The subscript \(m\) is for measurement uncertainty, but for this example we assume a constant measurement uncertainty: \[\textbf{R}_{1} = ... = \textbf{R}_{n}=\textbf{R}\]

So we have defined all the necessary blocks for the Kalman Gain, The state update equation and The covariance update equation.

Numerical example

Let us assume a vehicle moving in a straight line in the X direction with a constant velocity. After traveling 400 meters the vehicle turns right, with a turning radius of 300 meters. During the turning maneuver, the vehicle experiences acceleration due to the circular motion (angular acceleration).

# The measurements period (s)
deltaT = 1

# The random acceleration standard deviation (m/s^2)
sigma_a = 0.2

# The measurement error standard deviation (m)
sigma_xm = 3
sigma_ym = 3

# The state transition matrix F would be
vec1 <- c(1, 1, 0.5, 0, 0, 0)
vec2 <- c(0, 1, 1, 0, 0, 0)
vec3 <- c(0, 0, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1, 1, 0.5)
vec5 <- c(0, 0, 0, 0, 1, 1)
vec6 <- c(0, 0, 0, 0, 0, 1)
transMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6)

# The process noise matrix Q would be
vec1 <- c(1/4, 1/2, 1/2, 0, 0, 0)
vec2 <- c(1/2, 1, 1, 0, 0, 0)
vec3 <- c(1/2, 1, 1, 0, 0, 0)
vec4 <- c(0, 0, 0, 1/4, 1/2, 1/2)
vec5 <- c(0, 0, 0, 1/2, 1, 1)
vec6 <- c(0, 0, 0, 1/2, 1, 1)
processNoiseMat <- rbind(vec1, vec2, vec3, vec4, vec5, vec6) * 0.2^2

# The measurement covariance R would be
R <- rbind(c(20, 0), c(0, 20))

# Observation matrix
H <- rbind(c(1, 0, 0, 0, 0, 0), c(0, 0, 0, 1, 0, 0))
# The set of 35 noisy measurements
x <- c(301.5, 298.23, 297.83, 300.42, 301.94, 299.5, 305.98, 301.25, 299.73, 299.2, 298.62, 301.84, 299.6, 295.3, 299.3, 301.95, 296.3, 295.11, 295.12, 289.9, 283.51, 276.42, 264.22, 250.25, 236.66, 217.47, 199.75, 179.7, 160, 140.92, 113.53, 93.68, 69.71, 45.93, 20.87)

y <- c(-401.46, -375.44, -346.15, -320.2, -300.08, -274.12, -253.45, -226.4, -200.65, -171.62, -152.11, -125.19, -93.4, -74.79, -49.12, -28.73, 2.99, 25.65, 49.86, 72.87, 96.34, 120.4, 144.69, 168.06, 184.99, 205.11, 221.82, 238.3, 253.02, 267.19, 270.71, 285.86, 288.48, 292.9, 298.77)

zn <- cbind(x,y)

Iteration Zero

# Initialization
x00 <- c(0, 0, 0, 0, 0, 0) # as we do not know the vehicle location

# Since is a guess, we set a very high estimate uncertainty 
  # The high estimate uncertainty results in a high Kalman Gain by giving a high weight to the measurement.
P00 <- diag(c(500, 500, 500, 500, 500, 500))

Prediction

x10 <- transMat %*% x00; x10
##      [,1]
## vec1    0
## vec2    0
## vec3    0
## vec4    0
## vec5    0
## vec6    0
P10 <- transMat %*% P00 %*% t(transMat) + processNoiseMat; P10
##         vec1    vec2   vec3    vec4    vec5   vec6
## vec1 1125.01  750.02 250.02    0.00    0.00   0.00
## vec2  750.02 1000.04 500.04    0.00    0.00   0.00
## vec3  250.02  500.04 500.04    0.00    0.00   0.00
## vec4    0.00    0.00   0.00 1125.01  750.02 250.02
## vec5    0.00    0.00   0.00  750.02 1000.04 500.04
## vec6    0.00    0.00   0.00  250.02  500.04 500.04

First Iteration

# Step 1 - Measure
  # The measurement values:
z1 <- c(x[1], y[1]); z1
## [1]  301.50 -401.46
# Step 2 - Update
  # The Kalman Gain calculation:
k1 <- P10 %*% t(H) %*% solve(H %*% P10 %*% t(H) + R); k1
##           [,1]      [,2]
## vec1 0.9825329 0.0000000
## vec2 0.6550336 0.0000000
## vec3 0.2183562 0.0000000
## vec4 0.0000000 0.9825329
## vec5 0.0000000 0.6550336
## vec6 0.0000000 0.2183562

Therefore, we can see that the Kalman Gain for a position is 0.9921, which means that the weight of the first measurement is significantly higher than the weight of the estimation; that is the weight of the estimation is negligible.

# Estimate the current state:
x11 <- x10 + k1 %*% (z1 - H %*% x10); x11
##            [,1]
## vec1  296.23367
## vec2  197.49262
## vec3   65.83439
## vec4 -394.44766
## vec5 -262.96978
## vec6  -87.66127
# Update the current estimate uncertainty
P11 <- (diag(6) - k1 %*% H) %*% P10 %*% t(diag(6) - k1 %*% H) + k1 %*% R %*% t(k1); P11
##           vec1      vec2       vec3      vec4      vec5       vec6
## vec1 19.650658  13.10067   4.367123  0.000000   0.00000   0.000000
## vec2 13.100672 508.75171 336.268504  0.000000   0.00000   0.000000
## vec3  4.367123 336.26850 445.446590  0.000000   0.00000   0.000000
## vec4  0.000000   0.00000   0.000000 19.650658  13.10067   4.367123
## vec5  0.000000   0.00000   0.000000 13.100672 508.75171 336.268504
## vec6  0.000000   0.00000   0.000000  4.367123 336.26850 445.446590
# Step 3 - Predict
x21 <- transMat %*% x11; x21
##            [,1]
## vec1  526.64349
## vec2  263.32701
## vec3   65.83439
## vec4 -701.24808
## vec5 -350.63105
## vec6  -87.66127
P21 <- transMat %*% P11 %*% t(transMat) + processNoiseMat; P21
##           vec1      vec2     vec3      vec4      vec5     vec6
## vec1 1006.6110 1253.3656 563.3789    0.0000    0.0000   0.0000
## vec2 1253.3656 1626.7753 781.7551    0.0000    0.0000   0.0000
## vec3  563.3789  781.7551 445.4866    0.0000    0.0000   0.0000
## vec4    0.0000    0.0000   0.0000 1006.6110 1253.3656 563.3789
## vec5    0.0000    0.0000   0.0000 1253.3656 1626.7753 781.7551
## vec6    0.0000    0.0000   0.0000  563.3789  781.7551 445.4866
 # the prediction uncertainty is still very high

Generalization

multiKF <- function(transMat, processNoiseMat, H, R, iniX, iniP, zn){
  
  # Initialization
  x <- list(iniX)
  p <- list(iniP)
  k <- list(0)
  
  # Prediction
  x <- append(x, list(transMat %*% x[[1]]))
  p <- append(p, list(transMat %*% p[[1]] %*% t(transMat) + processNoiseMat))
  
  for (i in 2:dim(zn)[1]){
    # Measure
    z <- c(zn[i-1, 1], zn[i-1, 2])
    
    # Update
    k <- append(k, list(p[[length(p)]] %*% t(H) %*% solve(H %*% p[[length(p)]] %*% t(H) + R)))
    
    # Estimate the current state
    x <- append(x, list(x[[length(x)]] + k[[i]] %*% (z - H %*% x[[length(x)]])))
    
    # Update the current estimate uncertainty
    p <- append(p, list((diag(dim(k[[i]])[1]) - k[[i]] %*% H) %*% p[[length(p)]] %*% t(diag(dim(k[[i]])[1]) - k[[i]] %*% H) + k[[i]] %*% R %*% t(k[[i]])))
    
    # Predict
    x <- append(x, list(transMat %*% x[[length(x)]]))
    p <- append(p, list(transMat %*% p[[length(p)]] %*% t(transMat) + processNoiseMat))
  }
  return(list(x, p, k))
}

result <- multiKF(transMat, processNoiseMat, H, R, x00, P00, zn)
x <- result[[1]]
p <- result[[2]]
k <- result[[3]]

Visualization

Vehicle X-axis velocity

Since the absolute velocity of the mobile is 26, the true values of the velocity in the x axis will be: for the first 15s \(0\) and for the other 20s remaining \(-26\sin\theta(t)\)
\(\theta(t) = \frac{(t-15) \pi}{38}\)

# Take the velocity estimates in the x axis
estimatesX <- c()
for (i in seq(3, 70, by = 2)){
    estimatesX = append(estimatesX, list(x[[i]][2]))
}

trueValX <- c(rep(0, 15), -26*sin(((16:34 - 15) * pi) / 38))

# Dataframe with all the data
dataVx <- data.frame(
  recta = c(1:34),
  estimX = unlist(estimatesX),
  trueVal = trueValX
)
# Plot to compare the true value, measured values and estimates
plotVx <- ggplot(dataVx, aes(x = recta)) +
  geom_path(aes(y = trueVal, color = "True values")) +
  geom_point(aes(y = trueVal, color = "True values", shape = "True values")) +
  
  geom_path(aes(y = estimX, color = "Estimates")) +
  geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
  
  labs(title = "Vehicle X-axis velocity",
       x = "Time(s)",
       y = "Vx(m/s)",
       color = "") +
  
  scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")

ggplotly(plotVx) %>% config(displayModeBar = FALSE)

Vehicle Y-axis velocity

Since the absolute velocity of the mobile is 26, the true values of the velocity in the x axis will be: for the first 15s \(26\) and for the other 20s remaining \(\cos\theta(t)\)
\(\theta(t) = \frac{(t-15) \pi}{38}\)

# Take the velocity estimates in the y axis
estimatesY <- c()
for (i in seq(3, 70, by = 2)){
    estimatesY = append(estimatesY, list(x[[i]][5]))
}

# trueValY <- c(rep(1, 15), -cos(((16:34 - 400) * pi) / 600))
trueValY <- c(rep(26, 15), 26*cos(((16:34 - 15) * pi) / 38))

# Dataframe with all the data
dataVy <- data.frame(
  recta = c(1:34),
  estimY = unlist(estimatesY),
  trueVal = trueValY
)
# Plot to compare the true value, measured values and estimates
plotVy <- ggplot(dataVy, aes(x = recta)) +
  geom_path(aes(y = trueVal, color = "True values")) +
  geom_point(aes(y = trueVal, color = "True values", shape = "True values")) +
  
  geom_path(aes(y = estimY, color = "Estimates")) +
  geom_point(aes(y = estimY, color = "Estimates", shape = "Estimates")) +
  
  labs(title = "Vehicle Y-axis velocity",
       x = "Time(s)",
       y = "Vy(m/s)",
       color = "") +
  
  scale_color_manual(values = c("Estimates" = "red", "True values" = "green"), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "True values" = 18), name = "")

ggplotly(plotVy) %>% config(displayModeBar = FALSE)

Vehicle Position

The true values of the position in the x axis will be: for the first 15s \(300\) and for the other 20s remaining \(300\cos\theta(t)\)
\(\theta(t) = \frac{(t-15) \pi}{38}\)

The true values of the position in the x axis will be: the first value \(-400\) and then for the next 14s \(-400 + 26*t\) and for the other 20s remaining \(300\sin\theta(t)\)
\(\theta(t) = \frac{(t-15) \pi}{38}\)

# True values
trueValposX <- c(rep(300, 15), 300 * cos(((16:35 - 15) * pi) / 38))

trueValposY <- c(-400, -400 + 26*1:13, 0, 300 * sin(((16:35 - 15) * pi) / 38))


# Take the estimates in the x and y axis
estimatesX <- c()
estimatesY <- c()
for (i in seq(3, 70, by = 2)){
    estimatesX = append(estimatesX, list(x[[i]][1]))
    estimatesY = append(estimatesY, list(x[[i]][4]))
}
estimatesX = append(estimatesX, list(19.3))
estimatesY = append(estimatesY, list(297.79))

# Confidence ellipse for the probability of 95%:
scaleFactor <- sqrt(-2*log(1 - 0.95))

muValX <- c()
muValY <- c()
phiVal <- c()
kaVal <- c()
kbVal <- c()
for (i in seq(3, 70, by = 2)){
    # Get the covariance matrix for each estimation
    sigma2 <- rbind(c(p[[i]][1,1], 0),
                 c(0, p[[i]][4,4]))
    
    # Get the eigen values and eigen vectors
    res <- eigen(sigma2)
    
    # θ = arctan(vy/vx)
    phiVal <- append(phiVal, list(atan(res$vectors[2,1] / res$vectors[1,1])))
    
    # k * the highest eigenvalue square root
    kaVal <- append(kaVal, list(scaleFactor * sqrt(res$values[1])))
    
    # k * the second eigenvalue square root
    kbVal <- append(kbVal, list(scaleFactor * sqrt(res$values[2])))
    
    # Centre of the ellipse, each estimation in the x and y axis
    muValX <- append(muValX, list(c(x[[i]][1])))
    muValY <- append(muValY, list(c(x[[i]][4])))
}
muValX <- append(muValX, c(19.3))
muValY <- append(muValY, c(297.79))
phiVal <- append(phiVal, c(-1.570796))
kaVal <- append(kaVal, c(scaleFactor * sqrt(5)))
kbVal <- append(kbVal, c(scaleFactor * sqrt(5)))
# Dataframe with all the data
data <- data.frame(
  estimX = unlist(estimatesX), # only the estimates
  estimY = unlist(estimatesY),
  measurements.x = zn[,1],
  measurements.y = zn[,2],
  muValX <- unlist(muValX),
  muValY <- unlist(muValY),
  phiVal <- unlist(phiVal),
  kaVal <- unlist(kaVal),
  kbVal <- unlist(kbVal),
  trueValX = trueValposX,
  trueValY = trueValposY
)
# Plot to compare the true value, measured values and estimates
ggplot(data) +
  geom_path(aes(x = estimX, y = estimY, color = "Estimates")) +
  geom_point(aes(x = estimX, y = estimY, color = "Estimates", shape = "Estimates")) +
  
 geom_path(aes(x = measurements.x, y = measurements.y, color = "Measurements")) +
  geom_point(aes(x = measurements.x, y = measurements.y, color = "Measurements", shape = "Measurements")) +

  geom_ellipse(aes(x0 = muValX, y0 = muValY, a = kaVal, b = kbVal, angle = phiVal, color = "Interval", shape = 'Interval'), fill = 'yellow', alpha = 0.2) +

  geom_path(aes(x = trueValposX, y = trueValposY, color = "True values")) +
  geom_point(aes(x = trueValposX, y = trueValposY, color = "True values", shape = "True values")) +
  
  coord_fixed() + 
  
  labs(title = "Vehicle Position",
       x = "X(m)",
       y = "Y(m)",
       color = "") +
  
  xlim(c(0.00001, 310)) + 
  
  scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green", "Interval" = 'yellow'), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18, 'Interval' = 1), name = "")

The circles on the plot represent the 95% confidence ellipse. Since the x and y axes’ measurement errors are equal, the confidence ellipse is a circle.

Our Kalman Filter is designed for a constant acceleration model. Nevertheless, due to a properly chosen \(\sigma^2_{a}\) parameter it succeeds in tracking maneuvering vehicle, that is, the vehicle experiencing acceleration due to the circular motion - angular acceleration.

Example 10 – rocket altitude estimation

In this example, we will we estimate the altitude of a rocket, that is, we will design a two-dimensional Kalman Filter with a control input. We assume constant acceleration dynamics.

The rocket is equipped with an on-board altimeter that provides altitude measurements. In addition, the rocket is equipped with an accelerometer that measures the rocket’s acceleration, this serves as a control input to the Kalman Filter.

Accelerometers don’t sense gravity. Thus, we need to subtract the gravitational acceleration constant g from each accelerometer measurement.The accelerometer measurement at time step n is: \(a_{n} = \ddot{x} - g + \epsilon\) Where: \(\ddot{x}\) is the actual acceleration of the object (the second derivative of the object position), \(g\) is the gravitational acceleration constant (\(g\) = −9.8\(\frac{m}{s^2}\)) and \(\epsilon\) is the accelerometer measurement error.

Kalman Filter equations

For the state extrapolation equation: \(\begin{array}{ccc} \begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \end{bmatrix} = & \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix} & \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \end{bmatrix} + & \begin{bmatrix} 0.5\Delta t^2\\ \Delta t \end{bmatrix} & (a_{n} + g) \end{array}\)

For the covariance extrapolation equation: \[ P = \begin{bmatrix} p_{x} & p_{x\dot{x}} \\ p_{\dot{x}x} & p_{\dot{x}} \\ \end{bmatrix} \]

The process noise matrix, assuming a discrete noise model: \[\begin{align*} Q = & \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}}\\ \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}}\\ \end{bmatrix} \\ &= \epsilon^2 \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\ \frac{\Delta t^3}{2} & \Delta t^2 \\ \end{bmatrix} \end{align*}\]

The accelerometer error \(v\) is much lower than the system’s random acceleration; therefore, we use \(\epsilon^2\) as a multiplier of the process noise matrix. It makes our estimation uncertainty much lower.

In this example, the acceleration is handled by control input (therefore, it is not part of \(F\) matrix, and the \(Q\) matrix is \(2 × 2\)). Without an external control input, the process matrix would be \(3 × 3\).

The measurement equation:

The measurement provides only the altitude of the rocket: \[ z_{n} = \begin{bmatrix} x_{n, measured} \end{bmatrix} \]

The dimension of \(z_{n}\) is \(1 × 1\) and the dimension of \(x_{n}\) is \(2 × 1\), so the dimension of the observation matrix \(\textbf{H}\) is \(1 × 2\).

\[ \textbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} \]

The measurement uncertainty: \[ \textbf{R}_{n} = \begin{bmatrix} \sigma^2_{x_{m}} \end{bmatrix} \]

The subscript \(m\) means the “measurement”, but for this example we assume a constant measurement uncertainty: \[\textbf{R}_{1} = ... = \textbf{R}_{n}=\textbf{R}\]

So we have defined all the necessary blocks for the Kalman Gain, The state update equation and The covariance update equation.

Numerical example

Let us assume a vertically boosting rocket with constant acceleration. The rocket is equipped with an altimeter that provides altitude measurements and an accelerometer that serves as a control input.

# The measurements period: ∆t = 0.25s
# The rocket acceleration: x = 30 m/s^2
# The altimeter measurement error standard deviation: σ_xm = 20m
# The accelerometer measurement error standard deviation: ϵ = 0.1
# The state transition matrix F would be:
transF <- rbind(c(1, 0.25),
                c(0, 1))

# The control matrix G would be:
G <- c(0.5*0.25**2, 0.25)

# The process noise matrix Q would be:
Q <- 0.1**2 * rbind(c(0.25**4/4, 0.25**3/2),
           c(0.25**3/2, 0.25**2))

# The measurement uncertainty R would be:
R <- c(20**2)

# The observation matrix H
H <- cbind(1, 0)
# The set of 30 noisy measurements of the altitude hn and acceleration an:
hn <- c(6.43, 1.3, 39.43, 45.89, 41.44, 48.7, 78.06, 80.08, 61.77, 75.15, 110.39, 127.83, 158.75, 156.55, 213.32, 229.82, 262.8, 297.57, 335.69, 367.92, 377.19, 411.18, 460.7, 468.39, 553.9, 583.97, 655.15, 723.09, 736.85, 787.22)

an <- c(39.81, 39.67, 39.81, 39.84, 40.05, 39.85, 39.78, 39.65, 39.67, 39.78, 39.59, 39.87, 39.85, 39.59, 39.84, 39.9, 39.63, 39.59, 39.76, 39.79, 39.73, 39.93, 39.83, 39.85, 39.94, 39.86, 39.76, 39.86, 39.74, 39.94)

Initialization

# We don’t know the rocket’s location; we set the initial position and velocity to 0
x00 <- c(0,0)

# We assume 
u0 = 0

# Since our initial state vector is a guess, we set a very high estimate uncertainty
p00 <- rbind(c(500, 0), c(0, 500))

Prediction

x10 <- transF %*% x00 + G * u0; x10
##      [,1]
## [1,]    0
## [2,]    0
p10 <- transF %*% p00 %*% t(transF) + Q; p10
##          [,1]     [,2]
## [1,] 531.2500 125.0001
## [2,] 125.0001 500.0006

First iteration

# Update
k1 <- p10 %*% t(H) %*% solve(H %*% p10 %*% t(H) + R); k1
##           [,1]
## [1,] 0.5704698
## [2,] 0.1342283
x11 <- x10 + k1 %*% (hn[1] - H %*% x10); x11
##           [,1]
## [1,] 3.6681208
## [2,] 0.8630878
p11 <- (diag(dim(k1)[1]) - k1 %*% H) %*% p10 %*% t(diag(dim(k1)[1]) - k1 %*% H) + k1 %*% R %*% t(k1); p11
##           [,1]      [,2]
## [1,] 228.18792  53.69131
## [2,]  53.69131 483.22208
# Predict, the prediction uncertainty is still very high
x21 <- transF %*% x11 + G * (an[1] - 9.8); x21
##          [,1]
## [1,] 4.821705
## [2,] 8.365588
p21 <- transF %*% p11 %*% t(transF) + Q; p21
##          [,1]     [,2]
## [1,] 285.2350 174.4969
## [2,] 174.4969 483.2227

Generalization

multiKF2 <- function(transMat, processNoiseMat, H, R, G, iniX, iniP, hn, an){
  
  # Initialization
  x <- list(iniX)
  u <- list(0)
  p <- list(iniP)
  k <- list(0)
  
  # Prediction
  x <- append(x, list(transMat %*% x[[1]] + G * u[[1]]))
  p <- append(p, list(transMat %*% p[[1]] %*% t(transMat) + processNoiseMat))
  
  for (i in 2:length(hn)){
    # Update
    k <- append(k, list(p[[length(p)]] %*% t(H) %*% solve(H %*% p[[length(p)]] %*% t(H) + R)))
    
    # Estimate the current state
    x <- append(x, list(x[[length(x)]] + k[[i]] %*% (hn[i-1] - H %*% x[[length(x)]])))
    
    # Update the current estimate uncertainty
    p <- append(p, list((diag(dim(k[[i]])[1]) - k[[i]] %*% H) %*% p[[length(p)]] %*% t(diag(dim(k[[i]])[1]) - k[[i]] %*% H) + k[[i]] %*% R %*% t(k[[i]])))
    
    # Predict
    x <- append(x, list(transMat %*% x[[length(x)]] + G * (an[i-1] - 9.8)))
    p <- append(p, list(transMat %*% p[[length(p)]] %*% t(transMat) + processNoiseMat))
  }
  return(list(x, p, k))
}

result <- multiKF2(transF, Q, H, R, G, x00, p00, hn, an)
x <- result[[1]]
p <- result[[2]]
k <- result[[3]]

Visualization

Rocket Altitude

The true values of a vertically boosting rocket with constant acceleration \(30 m/s^2\) are calculated by \(x=ut+\frac{1}{2}at^2\) so assuming the rocket starts from rest \(u = 0\):

# True values for the first 7.25 seconds for each 0.25s
acceleration <- 30
trueValposX <- 0.5 * acceleration * seq(0, 7.25, by = 0.25)^2

# Take the estimates in the x axis
estimatesX <- c()
for (i in seq(3, 60, by = 2)){
    estimatesX = append(estimatesX, list(x[[i]][1]))
}
estimatesX = append(estimatesX, list(797.07))

# Take the estimates in the x and y axis
estimSigmaX <- c()
for (i in seq(3, 60, by = 2)){
    estimSigmaX = append(estimSigmaX, list(p[[i]][1,1]))
}
estimSigmaX = append(estimSigmaX, list(49.3))
# Dataframe with all the data
data <- data.frame(
  time = c(1:30),
  estimX = unlist(estimatesX), 
  measurements.x = hn,
  # the 95% confidence interval is given by [-1.96 sqrt(V); +1.96 sqrt(V)]
  interMax = unlist(estimatesX) + 1.96 * sqrt(unlist(estimSigmaX)),
  interMin = unlist(estimatesX) - 1.96 * sqrt(unlist(estimSigmaX)),
  trueValX = trueValposX
)
# Plot to compare the true value, measured values and estimates
plot1 <-  ggplot(data, aes(x = time)) +
 geom_path(aes(y = measurements.x, color = "Measurements")) +
  geom_point(aes(y = measurements.x, color = "Measurements", shape = "Measurements")) +
  
  geom_path(aes(y = trueValposX, color = "True values")) +
  geom_point(aes(y = trueValposX, color = "True values", shape = "True values")) +
  
  geom_path(aes(y = estimX, color = "Estimates")) +
  geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
  
  geom_line(aes(y = interMax, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
  geom_line(aes(y = interMin, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
  geom_ribbon(aes(ymin = interMin, ymax = interMax), fill = "yellow", alpha = 0.2) +
  
  labs(title = "Rocket Altitude",
       x = "Time(s)",
       y = "Altitude(m)",
       color = "") +
  
  scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green", "95% confidence interval" = 'yellow'), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18, '95% confidence interval' = 1), name = "")

ggplotly(plot1) %>% config(displayModeBar = FALSE)

We can see a good KF tracking performance and convergence.

Rocket Velocity

The true values of a vertically boosting rocket with constant acceleration \(30 m/s^2\) are calculated by \(v=u+at\) so assuming the rocket starts from rest \(u = 0\):

# True values for the first 7.25 seconds for each 0.25s
acceleration <- 30
trueValposV <- acceleration * seq(0, 7.25, by = 0.25)

# Take the estimates
estimatesV <- c()
for (i in seq(3, 60, by = 2)){
    estimatesV = append(estimatesV, list(x[[i]][2]))
}
estimatesV = append(estimatesV, list(215.7))

# Take the estimates in the x and y axis
estimSigmaV <- c()
for (i in seq(3, 60, by = 2)){
    estimSigmaV = append(estimSigmaV, list(p[[i]][2,2]))
}
estimSigmaV = append(estimSigmaV, list(2.6))
# Dataframe with all the data
dataV <- data.frame(
  time = c(1:30),
  estimX = unlist(estimatesV), 
  measurements.x = an,
  # the 95% confidence interval is given by [-1.96 sqrt(V); +1.96 sqrt(V)]
  interMax = unlist(estimatesV) + 1.96 * sqrt(unlist(estimSigmaV)),
  interMin = unlist(estimatesV) - 1.96 * sqrt(unlist(estimSigmaV)),
  trueValX = trueValposV
)
# Plot to compare the true value, measured values and estimates
plot2 <- ggplot(dataV, aes(x = time)) +
  geom_path(aes(y = trueValX, color = "True values")) +
  geom_point(aes(y = trueValX, color = "True values", shape = "True values")) +
  
  geom_path(aes(y = estimX, color = "Estimates")) +
  geom_point(aes(y = estimX, color = "Estimates", shape = "Estimates")) +
  
  geom_line(aes(y = interMax, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
  geom_line(aes(y = interMin, color = "95% confidence interval", shape = "95% confidence interval"), linetype = 1) +
  geom_ribbon(aes(ymin = interMin, ymax = interMax), fill = "yellow", alpha = 0.2) +
  
  labs(title = "Rocket Altitude",
       x = "Time(s)",
       y = "Altitude(m)",
       color = "") +
  
  scale_color_manual(values = c("Estimates" = "red", "Measurements" = "blue", "True values" = "green", "95% confidence interval" = 'yellow'), name = "") +
  scale_shape_manual(values = c("Estimates" = 19, "Measurements" = 15, "True values" = 18, '95% confidence interval' = 1), name = "")

ggplotly(plot2) %>% config(displayModeBar = FALSE)

It takes about 2.5 seconds to converge the estimates to the true rocket velocity. In the beginning, the estimated altitude is influenced by measurements, and it is not aligned well with the true rocket altitude since the measurements are very noisy. But as the KF converges, the noisy measurement has less influence, and the estimated altitude is well aligned with the true altitude.